%%
%
chang = 3.41-2* 0.275;
banjing = 16*0.55;
jiaodu = 1/banjing;
x_tou = zeros(2,420);
qiyu_zuobiao1 = zeros(224,420);
qiyu_zuobiao2 = zeros(224,420);
jiaodu_quanbu = zeros(420,1);

%计算头的位置坐标
for i = 0:420
    jiaodu1_huchang = 16*2*pi-jiaodu*i;
    jiaodu_quanbu(i+1,1) = jiaodu1_huchang; 
    x_tou(1,i+1) = 0.55*jiaodu1_huchang/(2*pi)*cos(jiaodu1_huchang);
    x_tou(2,i+1) = 0.55*jiaodu1_huchang/(2*pi)*sin(jiaodu1_huchang);
    leiji_theta = jiaodu1_huchang;
    bianjingwenti = 0.55*jiaodu1_huchang/(2*pi);
  for j = 2:223
    if j == 2
      tou_qiyu=2.86; 
    else
      tou_qiyu=1.65; 
    end
    %角度情况
      bianhua_heta = tou_qiyu/bianjingwenti;
      leiji_theta = leiji_theta+bianhua_heta;
      bianjingwenti = 1.7*leiji_theta/(2*pi);
    %每一个的位置
     qiyu_zuobiao1(j,i+1) = bianjingwenti * cos(leiji_theta);
     qiyu_zuobiao2(j,i+1) = bianjingwenti * sin(leiji_theta);
  end
end
%%
%求解速度
clc;
clear;
close all;
banjiang = 0.55 * 16 * 2 * pi;
luju = 0.55;
T_dexiangguanwenti = 3.41;
shendexiangguanwenti = 2.2;
suduqingkuang = 1;
zongshijian = 300;
sudu = 1;
x_weizhipositions = zeros(223,zongshijian+1);
y_weizhipositions = zeros(223,zongshijian+1);
zuizhongqiudesudu = zeros(223,zongshijian+1);
for sudu = 0:zongshijian
    theta_head = suduqingkuang * sudu / banjiang;
    r_abchead = banjiang + luju * theta_head;
    x_abchead = r_abchead * cos(theta_head);
    y_abchead = r_abchead * sin(theta_head);
    x_weizhipositions(1,sudu+1) = x_abchead;
    y_weizhipositions(1,sudu+1) = y_abchead;
    for i = 2:223
        L = (i-1) * shendexiangguanwenti + T_dexiangguanwenti;
        theta_i = theta_head - (L-T_dexiangguanwenti) / banjiang;
        r_i = r_abchead;
        x_i = r_i * cos(theta_i);
        y_i = r_i * sin(theta_i);
        x_weizhipositions(i,sudu+1) = x_i;
        y_weizhipositions(i,sudu+1) = y_i;
        v_i = luju * suduqingkuang / r_i;
        zuizhongqiudesudu(i,sudu+1) = v_i * 100;
    end
end


